A robot that can move but cannot decide how to move remains incomplete. Path planning lies at the intellectual core of autonomous robotics. It answers a deceptively simple question: given a starting configuration and a desired goal configuration, how should the robot move safely, efficiently, and reliably through its environment?
Planning transforms environmental knowledge into feasible physical motion. Using the map and localization information produced by SLAM (Simultaneous Localization and Mapping), a robot computes routes that avoid obstacles, satisfy physical constraints, and reach the target destination.
Unlike feedback control, which stabilizes motion and corrects small real-time errors, planning determines the structure of motion itself. It decides where the robot should go before low-level control systems determine how to execute that motion accurately.
Path planning algorithms enable autonomous mobile robots and multi-axis manipulators to evaluate complex physical environments and compute collision-free trajectories toward target goals. By integrating perceptual data from LiDAR, depth cameras, and tactile sensors, planners convert geometric workspace representations into configuration spaces (C-spaces). Algorithms ranging from discrete graph searches (A*, Dijkstra) to high-dimensional sampling frameworks (RRT, RRT*) and continuous non-linear trajectory optimization systematically balance computational complexity, path optimality, and kinematic constraints in real time.
Interactive Quick Review: Fundamentals of Path Planning
Test your understanding of global path planning versus local feedback control before exploring configuration space mathematics:
1. What is the fundamental functional difference between path planning and feedback control in autonomous robotics?
Answer: Path planning determines the global structure of motion itself (computing a continuous, collision-free geometric sequence of configurations from start to goal), whereas feedback control stabilizes that motion and corrects dynamic execution errors in real time.
2. How does path planning utilize spatial information generated by SLAM algorithms?
Answer: It converts the estimated environmental occupancy map and real-time robot localization pose produced by SLAM into a searchable free configuration space (\(\mathcal{C}_{\text{free}}\)) to evaluate feasibility and calculate obstacle-avoiding paths.
Glossary of Key Terminology
Essential foundational concepts defining robotic configuration manifolds, graph traversal, and kinodynamic motion planning:
Configuration Space Representation
Robotic planning does not typically operate directly within Cartesian physical space (\(\mathcal{W} \subset \mathbb{R}^2\) or \(\mathbb{R}^3\)). Instead, robotics relies on the concept of Configuration Space (\(\mathcal{C}\)), where each point \(\mathbf{q}\) corresponds to a unique geometric specification of the robot's spatial position and orientation.
For a planar rigid mobile robot navigating in \(SE(2)\):
$$\mathbf{q} = [x, y, \theta]^T \in \mathbb{R}^2 \times S^1$$For an \(n\)-joint articulated manipulator arm:
$$\mathbf{q} = [\theta_1, \theta_2, \dots, \theta_n]^T \in T^n$$where \(T^n\) denotes an \(n\)-dimensional torus representing combined revolute joint spaces.
The workspace obstacle space \(\mathcal{O} \subset \mathcal{W}\) maps to the configuration obstacle space \(\mathcal{C}_{\text{obs}}\):
$$\mathcal{C}_{\text{obs}} = \{ \mathbf{q} \in \mathcal{C} \mid \mathcal{R}(\mathbf{q}) \cap \mathcal{O} \neq \emptyset \}$$where \(\mathcal{R}(\mathbf{q})\) defines the volumetric footprint of the robot at pose \(\mathbf{q}\). For translation-only mobile platforms, \(\mathcal{C}_{\text{obs}}\) is formally constructed using the Minkowski difference:
$$\mathcal{C}_{\text{obs}} = \mathcal{O} \oplus (-\mathcal{R}(0)) = \{ \mathbf{p} + \mathbf{r} \mid \mathbf{p} \in \mathcal{O}, \, -\mathbf{r} \in \mathcal{R}(0) \}$$The open collision-free space is defined as \(\mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}\). The motion planning problem is to find a continuous path \(\tau: [0, 1] \to \mathcal{C}_{\text{free}}\) such that \(\tau(0) = \mathbf{q}_{\text{start}}\) and \(\tau(1) = \mathbf{q}_{\text{goal}}\). The structural kinematics enabling this transformation are detailed in Robot Kinematics and Dynamics.
Graph-Based Planning Algorithms
When \(\mathcal{C}_{\text{free}}\) is discretized into an occupancy grid, visibility graph, or Voronoi diagram, motion generation reduces to finding the optimal path through a topological graph \(G = (V, E)\), where vertices \(V\) represent collision-free states and directed edges \(E\) represent traversable transitions with positive weights \(w(u, v) > 0\).
Dijkstra’s Algorithm
Dijkstra's algorithm computes the shortest path from a start vertex to all other vertices by maintaining a priority queue ordered by cumulative path cost \(g(v)\). At each iteration, the node \(u\) with the lowest confirmed cost is expanded, and the cost to each neighbor \(v\) is updated via the relaxation rule:
$$g(v) = \min \left( g(v), \; g(u) + w(u, v) \right)$$While Dijkstra's algorithm guarantees global optimality over non-negative edge weights, it explores uniformly in all directions, incurring an \(\mathcal{O}(|E| + |V| \log |V|)\) computational overhead regardless of goal location.
A* Heuristic Search
A* improves search efficiency by guiding node expansion toward \(\mathbf{q}_{\text{goal}}\) using a heuristic estimation function. The priority evaluation function \(f(n)\) is defined as:
$$f(n) = g(n) + h(n)$$where:
- \(g(n)\): exact accumulated cost from start node \(\mathbf{q}_{\text{start}}\) to current node \(n\)
- \(h(n)\): estimated remaining cost from node \(n\) to goal node \(\mathbf{q}_{\text{goal}}\)
To guarantee path optimality, the heuristic function \(h(n)\) must be admissible (it must never overestimate the true remaining cost \(h^*(n)\)):
$$0 \le h(n) \le h^*(n) \quad \forall n \in V$$Furthermore, if \(h(n)\) satisfies the triangle inequality condition, it is consistent (monotonic):
$$h(u) \le w(u, v) + h(v)$$For Euclidean 2D planar grids, the standard Euclidean distance metric provides a consistent and admissible heuristic:
$$h(n) = \sqrt{(x_n - x_g)^2 + (y_n - y_g)^2}$$Graph search frameworks of this type form the core of path execution in automated warehousing, as covered in Warehouse Logistics and Autonomous Mobile Robots (AMRs).
Sampling-Based Planning in High Dimensions
When configuration space dimensions exceed 4 or 5 degrees of freedom (such as 6-DOF industrial manipulators or dual-arm cobots), discrete grid generation suffers from the exponential curse of dimensionality (\(\mathcal{O}(k^d)\) cells). Sampling-based algorithms bypass explicit \(\mathcal{C}_{\text{obs}}\) boundary construction by testing point samples for validity using fast collision checking.
Rapidly-Exploring Random Trees (RRT)
The standard RRT algorithm incrementally constructs a search tree \(\mathcal{T} = (V, E)\) biased toward unexplored regions of configuration space:
- Sample a random configuration \(\mathbf{q}_{\text{rand}} \sim \mathcal{U}(\mathcal{C})\).
- Find the nearest tree node: \(\mathbf{q}_{\text{near}} = \arg\min_{\mathbf{q} \in V} \|\mathbf{q} - \mathbf{q}_{\text{rand}}\|\).
- Extend a incremental step of length \(\epsilon\) toward \(\mathbf{q}_{\text{rand}}\): $$\mathbf{q}_{\text{new}} = \mathbf{q}_{\text{near}} + \epsilon \, \frac{\mathbf{q}_{\text{rand}} - \mathbf{q}_{\text{near}}}{\|\mathbf{q}_{\text{rand}} - \mathbf{q}_{\text{near}}\|}$$
- If the segment \([\mathbf{q}_{\text{near}}, \mathbf{q}_{\text{new}}] \subset \mathcal{C}_{\text{free}}\), insert \(\mathbf{q}_{\text{new}}\) into \(V\) and edge \((\mathbf{q}_{\text{near}}, \mathbf{q}_{\text{new}})\) into \(E\).
While RRT is probabilistically complete (finding a path if one exists as \(N \to \infty\)), the resulting paths are jagged and suboptimal, with a mathematical probability of zero of finding an optimal path.
RRT* (Optimal RRT)
RRT* achieves asymptotic optimality by augmenting standard RRT with two operations executed within a neighborhood ball \(B_r(\mathbf{q}_{\text{new}})\) of radius \(r = \gamma (\log |V| / |V|)^{1/d}\):
- Optimal Parent Selection: Rather than defaulting to \(\mathbf{q}_{\text{near}}\), RRT* connects \(\mathbf{q}_{\text{new}}\) to the neighbor \(\mathbf{q}_{\text{min}} \in B_r\) that minimizes cumulative cost from \(\mathbf{q}_{\text{start}}\): $$\text{cost}(\mathbf{q}_{\text{new}}) = \min_{\mathbf{q} \in B_r} \left( \text{cost}(\mathbf{q}) + \|\mathbf{q} - \mathbf{q}_{\text{new}}\| \right)$$
- Tree Rewiring: RRT* iterates over all nodes \(\mathbf{q}_{\text{near}} \in B_r\). If routing through \(\mathbf{q}_{\text{new}}\) provides a cheaper path from the start than their current parent connection, their incoming edges are rewired to \(\mathbf{q}_{\text{new}}\).
Through recursive local rewiring, RRT* guarantees that the path cost converges asymptotically to the theoretical optimal cost:
$$\mathbb{P} \left( \lim_{n \to \infty} c_n = c^* \right) = 1$$Engineering Challenges & Technical Solutions
Translating theoretical path planning algorithms to physical robotic hardware involves managing kinodynamic constraints, environmental uncertainty, and real-time execution limits:
The Challenge: Uniform random sampling struggles to navigate through narrow openings (such as doorways, pipe interiors, or tight obstacles). Because the volume of a narrow passage in configuration space is tiny relative to \(\mathcal{C}_{\text{free}}\), the probability of sampling valid configurations inside the opening approaches zero.
- Bridge Test Sampling: If two random configurations are in collision but their geometric midpoint is collision-free, the midpoint is retained, concentrating samples directly inside narrow corridors.
- Medial Axis / Voronoi Guidance: Sampling is biased along the generalized Voronoi diagram of the workspace, pulling search trees toward the maximum-clearance paths between obstacles.
The Challenge: Global planners (A*, Dijkstra) assume static cost maps. When unmapped obstacles (e.g., pedestrians, moving forklifts) block planned paths, full global replanning from scratch causes computational lag and trajectory pauses.
- Incremental Graph Replanners (D* Lite): D* Lite maintains cost estimates across graph edges, updating only nodes directly impacted by local obstacle changes without recomputing invariant portions of the map.
- Two-Layer Hierarchical Architectures: A global planner (A* or RRT*) computes a low-frequency geometric reference path (1–2 Hz), while a high-frequency local planner (such as the Dynamic Window Approach or Timed Elastic Bands running at 50 Hz) executes real-time collision avoidance.
The Challenge: Differential-drive and Ackermann car-like robots cannot translate sideways instantaneously. Piecewise-linear paths generated by standard geometric planners demand zero-radius turns, violating nonholonomic rolling contact constraints (\(\dot{y} \cos\theta - \dot{x} \sin\theta = 0\)).
- Reeds-Shepp & Dubins Path Steering: Geometric planners replace linear edge connections with combinations of maximum-curvature circular arcs and straight tangents.
- Hybrid A* Search: Operates in continuous \(SE(2)\) space by applying kinematic vehicle forward simulation to expand child nodes, ensuring every evaluated branch is executable by the steering system.
The Challenge: Raw graph and sampling-based paths consist of piecewise linear segments with discontinuous first derivatives (corners). Moving along corners requires infinite acceleration (\(a_c = v^2 / r \to \infty\)), producing severe mechanical chatter and actuator saturation.
- B-Spline & Polynomial Interpolation: Waypoints are parameterized into \(C^2\)-continuous cubic B-splines or quintic polynomials that guarantee bounded acceleration and jerk profiles.
- Post-Processing Shortcutting: Iterative ray casting between non-consecutive waypoints removes redundant detour vertices, shortening total path length prior to time scaling.
Kinodynamic Trajectory Optimization & Time Parameterization
A geometric path \(\mathbf{q}(s)\) defines spatial motion as a function of normalized arc length \(s \in [0, 1]\), but specifies no timing. To execute motion on physical hardware, the path must be mapped to a dynamic trajectory \(\mathbf{q}(t) = \mathbf{q}(s(t))\) that satisfies actuator limits.
Actuators impose hard physical velocity, acceleration, and torque bounds:
$$|\dot{\mathbf{q}}(t)| \le \mathbf{v}_{\text{max}}, \quad |\ddot{\mathbf{q}}(t)| \le \mathbf{a}_{\text{max}}, \quad |\boldsymbol{\tau}(t)| \le \boldsymbol{\tau}_{\text{max}}$$The time-scaling derivatives are obtained using the chain rule:
$$\dot{\mathbf{q}}(t) = \mathbf{q}'(s) \dot{s}(t)$$ $$\ddot{\mathbf{q}}(t) = \mathbf{q}'(s) \ddot{s}(t) + \mathbf{q}''(s) \dot{s}^2(t)$$where \(\mathbf{q}'(s) = \frac{d\mathbf{q}}{ds}\) and \(\mathbf{q}''(s) = \frac{d^2\mathbf{q}}{ds^2}\) represent geometric path tangents and curvatures.
In trajectory optimization (e.g., CHOMP, TrajOpt), motion planning is formulated as a non-linear programming (NLP) problem:
$$\min_{\mathbf{q}(t)} \int_0^T \left( w_{\text{smooth}} \|\ddot{\mathbf{q}}(t)\|^2 + w_{\text{obs}} c_{\text{cost}}(\mathbf{q}(t)) \right) dt$$ $$\text{subject to} \quad \mathbf{q}(0) = \mathbf{q}_{\text{start}}, \quad \mathbf{q}(T) = \mathbf{q}_{\text{goal}}, \quad d(\mathcal{R}(\mathbf{q}(t)), \mathcal{O}) \ge d_{\text{safe}}$$These formulations integrate planning directly with the dynamics frameworks discussed in Advanced Control Systems in Robotics.
Interactive Quick Review: Sampling, Optimization & Trajectories
Reinforce key concepts regarding sampling-based algorithms, time parameterization, and asymptotic optimality before reviewing the FAQ:
1. Why are sampling-based planners like RRT and RRT* preferred over graph-based planners for high-dimensional manipulators?
Answer: High-dimensional configuration spaces suffer from the exponential curse of dimensionality under grid discretization (\(\mathcal{O}(k^d)\)); sampling-based planners explore C-space incrementally via random sampling without needing full spatial grid construction.
2. What is the fundamental difference between a geometric path \(\mathbf{q}(s)\) and an executable trajectory \(\mathbf{q}(t)\)?
Answer: A geometric path defines spatial coordinate progression independent of time, whereas an executable trajectory parameterizes the path over time (\(s(t)\)), defining velocity, acceleration, and jerk profiles to satisfy actuator dynamic constraints.
External References and Further Reading
To explore foundational literature, research algorithms, and motion planning frameworks, consult the following authoritative sources:
- Planning Algorithms (Steven M. LaValle) – A foundational academic textbook covering configuration space theory, nonholonomic motion planning, and sampling-based algorithms.
- Open Motion Planning Library (OMPL) – An open-source C++ framework containing robust implementations of sampling-based planners including RRT, RRT*, KPIECE, and PRM.
- IEEE Transactions on Robotics (T-RO) – Peer-reviewed academic journal publishing research on kinodynamic planning, trajectory optimization, and multi-agent routing.
Frequently Asked Questions about Path Planning Algorithms for Robots
What is the difference between path planning and trajectory planning?
Path planning generates an obstacle-free geometric curve connecting start to goal without specifying timing. Trajectory planning parameterizes this geometric curve with respect to time (\(\mathbf{q}(t) = \mathbf{q}(s(t))\)), calculating velocity, acceleration, and torque profiles to ensure physical execution within motor limits.
What is an admissible heuristic in A* graph search?
An admissible heuristic \(h(n)\) is a cost estimator that never overestimates the true minimal cost remaining to reach the goal (\(h(n) \le h^*(n)\)). Admissibility guarantees that A* will find an optimal path without terminating prematurely on suboptimal branches.
Why does standard RRT fail to find optimal paths, and how does RRT* fix this?
Standard RRT stops expanding once a valid path connects to the goal, leaving suboptimal jagged routes. RRT* introduces optimal neighborhood parent selection and local tree rewiring, continuously shortening path length and guaranteeing asymptotic convergence to the theoretical optimal cost (\(\lim_{n \to \infty} c_n = c^*\)).
What is Configuration Space (C-Space), and why is it useful?
Configuration space is a mathematical manifold where the robot is represented as a single dimensionless point \(\mathbf{q}\), and physical obstacles are expanded into \(\mathcal{C}\)-obstacles using Minkowski operations. This reduces complex 3D polygon-to-mesh intersection testing to checking whether a single point falls inside \(\mathcal{C}_{\text{obs}}\).
End-of-Page Exercises: Review, Thought-Provoking & Engineering Problems
Section 1: Foundational Review Questions & Answers
1. What does configuration space (\(\mathcal{C}\)-space) represent in robotics?
Answer: A manifold where every coordinate point \(\mathbf{q}\) uniquely defines the complete spatial position and orientation of the robot mechanism.
2. What is the fundamental update equation for Dijkstra's graph search algorithm?
Answer: \(g(v) = \min(g(v), g(u) + w(u, v))\), relaxing the shortest known path cost from start to node \(v\) through neighbor \(u\).
3. In A* search, what condition must the heuristic function \(h(n)\) satisfy to guarantee path optimality?
Answer: Admissibility, meaning \(h(n)\) must never overestimate the true remaining cost to reach the goal (\(h(n) \le h^*(n)\)).
4. How does RRT generate a new configuration candidate \(\mathbf{q}_{\text{new}}\) during tree expansion?
Answer: By sampling a random configuration \(\mathbf{q}_{\text{rand}}\), identifying the nearest existing tree node \(\mathbf{q}_{\text{near}}\), and extending a distance \(\epsilon\) along the vector connecting them.
5. What property distinguishes RRT* from standard RRT?
Answer: Asymptotic optimality (\(\lim_{n \to \infty} c_n = c^*\)), achieved through optimal local parent selection and tree rewiring.
6. What inequality defines a collision-free robot configuration \(\mathbf{q}\)?
Answer: \(d(\mathcal{R}(\mathbf{q}), \mathcal{O}_i) > 0\), stating that the Euclidean distance between robot geometry and all obstacles is strictly positive.
7. How is the dimension of the joint configuration space calculated for a team of \(N\) independent robots?
Answer: \(\text{dim}(\mathcal{C}) = \sum_{i=1}^{N} \text{dim}(\mathcal{C}_i)\), summing individual subsystem dimensions.
8. What constraint guarantees real-time feasibility for planning algorithms running on embedded hardware?
Answer: Total computation duration must not exceed the execution control cycle deadline: \(T_{\text{plan}} \le T_{\text{deadline}}\).
9. What limits differentiate geometric path feasibility from dynamic trajectory execution?
Answer: Maximum joint/wheel velocities (\(|\dot{\mathbf{q}}(t)| \le \mathbf{v}_{\text{max}}\)) and maximum accelerations (\(|\ddot{\mathbf{q}}(t)| \le \mathbf{a}_{\text{max}}\)).
10. Name five university mathematics subjects essential for mastering motion planning algorithms.
Answer: Graph theory, multivariable calculus, linear algebra, non-linear optimization theory, and numerical methods.
Section 2: Analytical & Thought-Provoking Questions
1. How does the choice of heuristic function \(h(n)\) influence search speed and path optimality in A*?
Answer: When \(h(n) = 0\), A* degrades into Dijkstra's algorithm, expanding uniformly in all directions with guaranteed optimality at high computational cost. If \(h(n) = h^*(n)\), A* expands only nodes on the optimal path. If \(h(n)\) overestimates true cost (inadmissible), A* loses guaranteed optimality but expands fewer nodes, behaving like greedy best-first search.
2. Why do nonholonomic constraints break standard Euclidean grid distance metrics?
Answer: A car-like vehicle cannot translate sideways. Two configurations with identical \((x, y)\) coordinates but opposing headings have small Euclidean distance, yet connecting them requires multi-point parallel parking maneuvers. Distance metrics in nonholonomic spaces must reflect dynamic steering feasibility (such as Dubins or Reeds-Shepp curves).
3. How does local rewiring in RRT* transform an asymptotically suboptimal random tree into an optimal path?
Answer: Whenever a new configuration \(\mathbf{q}_{\text{new}}\) is added, RRT* checks neighbor nodes within a local radius ball. If routing neighbors through \(\mathbf{q}_{\text{new}}\) produces a lower path cost from \(\mathbf{q}_{\text{start}}\) than their current parent connections, incoming edges are rewired. This local optimization propagates across generations, asymptotically shedding path suboptimality.
4. Why is continuous trajectory optimization prone to getting trapped in local minima?
Answer: Non-linear trajectory optimization balances smoothness and obstacle clearance costs. In cluttered environments, obstacles introduce non-convex barriers. Gradient descent initialized on the wrong side of an obstacle cannot pass through high-cost \(\mathcal{C}_{\text{obs}}\) regions, trapping the trajectory in a suboptimal or colliding local minimum.
5. How does centralized multi-robot planning differ computationally from decentralized planning?
Answer: Centralized planning computes paths in a combined high-dimensional configuration space (\(\text{dim} = \sum \text{dim}(\mathcal{C}_i)\)), guaranteeing global optimality and collision avoidance, but suffering from exponential complexity. Decentralized planning lets each robot plan independently in its local configuration space, treating other agents as dynamic obstacles; this scales to large fleets but can lead to deadlocks.
6. What mathematical problems arise when time-parameterizing a geometric path near a sharp 90-degree corner?
Answer: A sharp corner has a discontinuous tangent vector, resulting in infinite curvature (\(\kappa \to \infty\)). Traversing this corner at non-zero velocity requires infinite normal acceleration (\(a_n = v^2 \kappa \to \infty\)), which violates motor limits. Planners must either bring velocity to zero at the vertex or smooth the geometry using continuous-curvature splines.
7. Why is hierarchical collision checking faster than brute-force mesh triangle evaluation?
Answer: Brute-force checking tests every polygon triangle on the robot against every obstacle triangle (\(\mathcal{O}(M \times N)\)). Hierarchical models enclose components inside bounding volume trees (spheres or oriented bounding boxes). If two top-level bounding volumes do not intersect, thousands of internal mesh tests are rejected in a single check.
8. How does the Dynamic Window Approach (DWA) avoid unexpected moving obstacles in real time?
Answer: DWA samples a local window of achievable forward and rotational velocities \((v, \omega)\) constrained by motor acceleration limits. It simulates forward circular trajectories over short time horizons, selecting the velocity pair that maximizes heading alignment and velocity while guaranteeing the robot can stop before colliding.
9. What role does path smoothing play after an RRT algorithm returns a raw piecewise-linear path?
Answer: Raw sampling paths consist of jagged, random linear segments. Post-processing algorithms apply shortcutting (connecting non-adjacent waypoints directly if collision-free) and cubic B-spline interpolation, eliminating redundant detours and producing \(C^2\)-continuous trajectories that conserve actuator energy.
10. How do dynamic environmental changes violate the static global planning assumption of Dijkstra and A* planners?
Answer: Static planners assume graph edge costs remain constant during navigation. Moving obstacles invalidate edge weights, making the precomputed route untraversable. Systems use incremental algorithms like D* Lite, which update only the graph nodes affected by obstacle changes without recomputing the entire search tree.
11. Why is goal bias sampling introduced into RRT random configuration generation?
Answer: Uniform sampling explores all C-space directions equally, spending time expanding branches away from the destination. Setting a 5% to 10% probability where \(\mathbf{q}_{\text{rand}} = \mathbf{q}_{\text{goal}}\) steers tree growth toward the target while preserving the ability to navigate around large obstacles.
12. How does the Minkowski sum construct C-obstacles for polygonal mobile robots?
Answer: C-obstacles are constructed by sweeping the inverted robot geometry around workspace obstacle boundaries: \(\mathcal{C}_{\text{obs}} = \mathcal{O} \oplus (-\mathcal{R})\). This inflates obstacles by the robot's physical dimensions, allowing the robot to be treated as a single dimensionless point during graph traversal.
Section 3: Numerical Engineering Problems & Solutions
In an A* grid planner, a mobile robot evaluates a candidate node \(n\) located at grid coordinate \((4, 7)\). The start node is at \((0, 0)\) and cumulative path cost from start to node \(n\) is \(g(n) = 8.5\text{ meters}\). The goal coordinate is at \((10, 15)\). Using Euclidean distance as an admissible heuristic \(h(n)\), calculate \(h(n)\) and determine the total A* node priority cost \(f(n) = g(n) + h(n)\).
1. Coordinate differences: $$\Delta x = 10 - 4 = 6\text{ m}, \quad \Delta y = 15 - 7 = 8\text{ m}$$ 2. Euclidean heuristic: $$h(n) = \sqrt{\Delta x^2 + \Delta y^2} = \sqrt{6^2 + 8^2} = \sqrt{36 + 64} = \sqrt{100} = 10.0\text{ meters}$$ 3. Total A* cost: $$f(n) = g(n) + h(n) = 8.5\text{ m} + 10.0\text{ m} = 18.5\text{ meters}$$ Final Answer: Heuristic distance \(h(n)\) is \(10.0\text{ m}\) and total evaluation priority cost \(f(n)\) is \(18.5\text{ m}\).
An RRT planner selects a nearest tree node at \(\mathbf{q}_{\text{near}} = [2.0, 3.0]^T\text{ meters}\) and samples a random configuration at \(\mathbf{q}_{\text{rand}} = [8.0, 11.0]^T\text{ meters}\). The extension step size limit is set to \(\epsilon = 1.5\text{ meters}\). Calculate the vector difference \((\mathbf{q}_{\text{rand}} - \mathbf{q}_{\text{near}})\), its Euclidean norm, and determine the exact coordinates of the new tree node \(\mathbf{q}_{\text{new}}\).
1. Vector displacement: $$\mathbf{v} = \mathbf{q}_{\text{rand}} - \mathbf{q}_{\text{near}} = \begin{bmatrix} 8.0 - 2.0 \\ 11.0 - 3.0 \end{bmatrix} = \begin{bmatrix} 6.0 \\ 8.0 \end{bmatrix}\text{ m}$$ 2. Vector norm: $$\|\mathbf{v}\| = \sqrt{6.0^2 + 8.0^2} = \sqrt{36.0 + 64.0} = 10.0\text{ meters}$$ 3. Compute new configuration: $$\mathbf{q}_{\text{new}} = \mathbf{q}_{\text{near}} + \epsilon \frac{\mathbf{v}}{\|\mathbf{v}\|} = \begin{bmatrix} 2.0 \\ 3.0 \end{bmatrix} + 1.5 \begin{bmatrix} 0.6 \\ 0.8 \end{bmatrix} = \begin{bmatrix} 2.0 + 0.9 \\ 3.0 + 1.2 \end{bmatrix} = \begin{bmatrix} 2.9 \\ 4.2 \end{bmatrix}\text{ m}$$ Final Answer: The extended tree node coordinate is \(\mathbf{q}_{\text{new}} = [2.9, 4.2]^T\text{ meters}\).
A robot arm joint trajectory is parameterized as \(q(t) = 0.5 t^2 + 0.2 t\text{ radians}\) over the time interval \(t \in [0, 4.0]\text{ seconds}\). The joint motor has a maximum allowable velocity of \(v_{\text{max}} = 4.0\text{ rad/s}\). Derive the velocity equation \(\dot{q}(t)\), calculate the peak velocity during motion, and verify whether the physical constraint \(|\dot{q}(t)| \le v_{\text{max}}\) is satisfied.
1. Differentiate position with respect to time: $$\dot{q}(t) = \frac{d}{dt} (0.5 t^2 + 0.2 t) = 1.0 t + 0.2\text{ rad/s}$$ 2. Evaluate maximum velocity at \(t = 4.0\text{ s}\): $$\dot{q}(4.0) = (1.0 \times 4.0) + 0.2 = 4.2\text{ rad/s}$$ 3. Compare with physical boundary: $$4.2\text{ rad/s} > 4.0\text{ rad/s} \Rightarrow \text{Constraint violated}$$ Final Answer: Peak velocity is \(4.2\text{ rad/s}\), which violates the \(4.0\text{ rad/s}\) safety limit.
A centralized multi-agent logistics controller coordinates paths for \(N_1 = 4\) differential mobile robots (\(\text{dim}(\mathcal{C}_{\text{mobile}}) = 3\) each) and \(N_2 = 2\) articulated industrial manipulators (\(\text{dim}(\mathcal{C}_{\text{arm}}) = 6\) each). Calculate the total topological dimension of the joint configuration space \(\text{dim}(\mathcal{C}_{\text{total}})\).
1. Sum of mobile robot dimensions: $$\text{dim}(\mathcal{C}_{\text{fleet}}) = 4 \times 3 = 12$$ 2. Sum of manipulator arm dimensions: $$\text{dim}(\mathcal{C}_{\text{arms}}) = 2 \times 6 = 12$$ 3. Total joint dimension: $$\text{dim}(\mathcal{C}_{\text{total}}) = \text{dim}(\mathcal{C}_{\text{fleet}}) + \text{dim}(\mathcal{C}_{\text{arms}}) = 12 + 12 = 24$$ Final Answer: Total combined configuration space dimension is \(24\text{ dimensions}\).
An autonomous ground vehicle replanner executes an onboard trajectory update loop with a hard real-time execution deadline of \(T_{\text{deadline}} = 50\text{ milliseconds}\). Benchmarks indicate that tree sampling takes \(32\text{ ms}\), local rewiring takes \(12\text{ ms}\), and collision verification takes \(9\text{ ms}\). Calculate total computation time \(T_{\text{plan}}\) and determine whether \(T_{\text{plan}} \le T_{\text{deadline}}\) is satisfied.
1. Total computation duration: $$T_{\text{plan}} = 32\text{ ms} + 12\text{ ms} + 9\text{ ms} = 53\text{ ms}$$ 2. Real-time condition check: $$53\text{ ms} > 50\text{ ms} \Rightarrow \text{Execution deadline missed}$$ Final Answer: Total planning duration is \(53\text{ ms}\), which exceeds the \(50\text{ ms}\) deadline by \(3\text{ ms}\).
A point robot is positioned at \(\mathbf{q} = [5.0, 8.0]^T\text{ meters}\). A cylindrical obstacle \(\mathcal{O}_1\) is centered at \([2.0, 4.0]^T\text{ meters}\) with radius \(r = 2.0\text{ meters}\). Calculate the Euclidean center-to-center distance, compute the minimal clearance \(d(\mathcal{R}(\mathbf{q}), \mathcal{O}_1)\), and confirm whether \(\mathbf{q} \in \mathcal{C}_{\text{free}}\).
1. Center-to-center Euclidean distance: $$d_{\text{center}} = \sqrt{(5.0 - 2.0)^2 + (8.0 - 4.0)^2} = \sqrt{3.0^2 + 4.0^2} = \sqrt{9.0 + 16.0} = 5.0\text{ meters}$$ 2. Minimal surface clearance: $$d(\mathcal{R}(\mathbf{q}), \mathcal{O}_1) = d_{\text{center}} - r = 5.0\text{ m} - 2.0\text{ m} = 3.0\text{ meters}$$ 3. Collision check: $$3.0\text{ m} > 0 \Rightarrow \mathbf{q} \in \mathcal{C}_{\text{free}}$$ Final Answer: Minimum clearance is \(3.0\text{ meters}\); the configuration is collision-free.
A mobile robot traverses a geometric path parameterized by arc length \(s(t) = 1.5 t^2\text{ meters}\) over the interval \(t \in [0, 2.0]\text{ seconds}\). Calculate the path velocity \(v(t) = \frac{ds}{dt}\) at \(t = 2.0\text{ seconds}\) and the total distance traversed during that period.
1. Compute instantaneous velocity: $$v(t) = \frac{ds}{dt} = \frac{d}{dt} (1.5 t^2) = 3.0 t\text{ m/s}$$ 2. Evaluate at \(t = 2.0\text{ s}\): $$v(2.0) = 3.0 \times 2.0 = 6.0\text{ m/s}$$ 3. Total distance traversed: $$s(2.0) = 1.5 \times (2.0)^2 = 1.5 \times 4.0 = 6.0\text{ meters}$$ Final Answer: Linear velocity at \(t = 2.0\text{ s}\) is \(6.0\text{ m/s}\), with a traversed distance of \(6.0\text{ meters}\).
In a roadmap graph, the current shortest known cost from start to node \(u\) is \(g(u) = 14.2\text{ meters}\). The current recorded cost to neighbor node \(v\) is \(g(v) = 22.0\text{ meters}\). The connecting edge weight is \(w(u, v) = 5.3\text{ meters}\). Apply the relaxation update rule \(g(v) = \min(g(v), g(u) + w(u, v))\) and state whether \(g(v)\) is updated.
1. Candidate path cost: $$g_{\text{candidate}} = g(u) + w(u, v) = 14.2\text{ m} + 5.3\text{ m} = 19.5\text{ meters}$$ 2. Evaluate relaxation rule: $$g_{\text{new}}(v) = \min(22.0\text{ m}, 19.5\text{ m}) = 19.5\text{ meters}$$ 3. Update status: $$19.5\text{ m} < 22.0\text{ m} \Rightarrow \text{Distance value is updated}$$ Final Answer: Cost is updated to \(g(v) = 19.5\text{ meters}\).
A mobile platform moves along a straight path of length \(L = 9.0\text{ meters}\) from rest (\(v_0 = 0\)) to rest (\(v_f = 0\)). Motor acceleration and deceleration are capped at \(a_{\text{max}} = 1.0\text{ m/s}^2\). Assuming a symmetric triangular velocity profile, calculate the peak midpoint velocity \(v_{\text{peak}}\) and total motion time \(T\).
1. Midpoint distance: $$d_{\text{half}} = \frac{L}{2} = \frac{9.0}{2} = 4.5\text{ meters}$$ 2. Peak velocity calculation: $$v_{\text{peak}}^2 = 2 a_{\text{max}} d_{\text{half}} = 2 \times 1.0 \times 4.5 = 9.0\text{ m}^2/\text{s}^2 \Rightarrow v_{\text{peak}} = \sqrt{9.0} = 3.0\text{ m/s}$$ 3. Time to midpoint: $$t_{\text{acc}} = \frac{v_{\text{peak}}}{a_{\text{max}}} = \frac{3.0}{1.0} = 3.0\text{ seconds}$$ 4. Total trajectory duration: $$T = 2 t_{\text{acc}} = 2 \times 3.0 = 6.0\text{ seconds}$$ Final Answer: Peak velocity is \(3.0\text{ m/s}\) and total motion duration is \(6.0\text{ seconds}\).
An RRT* benchmark evaluation records path cost across sample counts. At \(n_1 = 1,000\) iterations, path cost is \(c_{1000} = 15.6\text{ meters}\). At \(n_2 = 10,000\) iterations, tree rewiring reduces cost to \(c_{10000} = 12.3\text{ meters}\). If theoretical optimal cost is \(c^* = 12.0\text{ meters}\), calculate the initial error \(\Delta c_1\), final error \(\Delta c_2\), and the percentage error reduction.
1. Error at \(n = 1,000\): $$\Delta c_1 = 15.6\text{ m} - 12.0\text{ m} = 3.6\text{ meters}$$ 2. Error at \(n = 10,000\): $$\Delta c_2 = 12.3\text{ m} - 12.0\text{ m} = 0.3\text{ meters}$$ 3. Percentage error reduction: $$\% \text{ Reduction} = \frac{\Delta c_1 - \Delta c_2}{\Delta c_1} \times 100 = \frac{3.6 - 0.3}{3.6} \times 100 = \frac{3.3}{3.6} \times 100 \approx 91.67\%$$ Final Answer: Suboptimality drops from \(3.6\text{ m}\) to \(0.3\text{ m}\), achieving a \(91.67\%\) error reduction.
An engineer evaluates grid discretization for a 6-DOF industrial articulated arm by discretizing each revolute joint \(\theta_i\) into \(k = 100\) uniform angular bins. Calculate the total number of cells \(N_{\text{cells}} = k^d\) in the 6D configuration space grid.
1. Configuration cell formula: $$N_{\text{cells}} = k^d = 100^6$$ 2. Simplify powers of 10: $$N_{\text{cells}} = (10^2)^6 = 10^{12} = 1,000,000,000,000\text{ cells}$$ Final Answer: Discretization produces \(1\text{ trillion cells}\) (\(10^{12}\)), demonstrating why sampling-based algorithms are preferred for high-DOF systems.
An optimization-based planner minimizes trajectory kinetic effort modeled by the objective functional \(J = \int_0^2 (\dot{q}(t))^2 dt\). For a constant joint velocity of \(\dot{q}(t) = 3.0\text{ rad/s}\) over duration \(T = 2.0\text{ seconds}\), calculate the value of cost functional \(J\).
1. Square the velocity term: $$(\dot{q}(t))^2 = (3.0\text{ rad/s})^2 = 9.0\text{ rad}^2/\text{s}^2$$ 2. Integrate across the time horizon: $$J = \int_0^{2.0} 9.0 \, dt = [9.0 t]_0^{2.0} = 9.0 \times 2.0 = 18.0$$ Final Answer: The trajectory optimization cost is \(J = 18.0\).
Path Planning Algorithms for Robots — Summary & Wrap-Up
Path planning algorithms form the deliberative intelligence of autonomous mobile robots and multi-axis manipulators. By translating physical environments into configuration spaces (\(\mathcal{C}\)-space), engineers leverage discrete graph searches (A*, Dijkstra), high-dimensional sampling frameworks (RRT, RRT*), dynamic trajectory optimization, and multi-robot coordination to generate collision-free, energy-efficient, and kinematically feasible trajectories across complex operational domains.
The Engineering Chain Behind Autonomous Robot Motion
Autonomous movement does not emerge from a single algorithm. It is the result of several engineering layers working together: mechanical systems generate physical motion, mathematical models describe how motion behaves, sensors interpret the surrounding world, localization determines where the robot is, planning decides where it should go, and control systems execute those decisions with precision.
Actuation and Mechanical Design in Robotics
Motors, transmissions, structural components, and compliant mechanisms give robots their physical ability to move, grasp, and interact with the physical world.
Robot Kinematics and Dynamics
Kinematic models describe geometric motion of joints and links, while dynamic models explain how forces and torques produce acceleration and mechanical response.
Sensors and Perception
Cameras, LiDAR, radar, and other perception sensors allow robots to detect obstacles, interpret complex surroundings, and transform raw measurements into structured environmental maps.
SLAM: Localization and Mapping
Simultaneous Localization and Mapping algorithms estimate the robot's pose while constructing a map of an unknown environment from sensor streams.
Path Planning Algorithms
The present module focuses on deliberative motion planning — the computational process that determines a safe, efficient route from the robot's current configuration to a desired goal.
Robot Control Systems
Control algorithms convert planned spatial trajectories into actuator commands while continuously correcting dynamic tracking deviations through closed-loop feedback.